/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
# example client commanding coordinatedrive with swig wrapped c++ library in python
import sys, time
sys.path.append("../../../interfaces/wrap/Python/lib")
sys.path.append("../../../../GIMI/wrap/Python/lib")
import CoordinateDriveClient
#import timestamp
import gimi
if sys.argv.__len__() != 3:
	print "Usage: python %s clientName serverName"%sys.argv[0]
	sys.exit()
g = gimi.GIMI()
cdc = CoordinateDriveClient.CCoordinateDriveClient(g, 0)
print "connecting... ",

ret = g.connectToHubEx("130.233.120.103", 50006, sys.argv[1], 5000)
#ret = g.connectToHubEx("asrobo.hut.fi", 50002, sys.argv[1])
#ret = g.connectToHubEx("asrobo.hut.fi", 50002, "testCli")
#print CoordinateDriveClient.GetErrorStr(ret)
print "set target"
cdc.SetDefaultTarget(sys.argv[2])
#cdc.SetDefaultTarget("codum")
print "open...",
ret = cdc.Open()
print CoordinateDriveClient.GetErrorStr(ret)
del ret

#clear coordinates
print "remove coordinates"
ret = cdc.RemoveCoordinates()
print "coordinates removal succesfull? ",
print ret


#set coordinate (x,y,a,bRel,bTargetPoint, bWaitPoint)
print "setting coordinate"
ret = cdc.SetCoordinate(2.0, 1.0, 0.5, False, False, False)
ret = cdc.SetCoordinate(0.0, 0.0, 0.0, False, False, False)
print "coordinate set succesfull? ",
print ret


#start
print "starting to drive"
ret = cdc.SetStart()
print "starting to drive succesful?",
print ret

#stop
print "stoping to drive"
ret = cdc.SetStop()
print "stoping to drive succesful?",
print ret

#get path length
print "getting path length"
i,j=0.,0.
ret = cdc.GetPathLength(i,j)
print ret

print "create array container"
cont = CoordinateDriveClient.TCoordinateContainer()
tStamp = CoordinateDriveClient.TTimestamp()
print "getting coordinates... "
ret = cdc.GetCoordinates(cont, tStamp, None, 1000)
#ret = cdc.GetCoordinates(a.points, 1000)
print "coordinate get succesfull? ",
print ret

#number of coordinates
print "getting number of coordinates"
n = cont.GetArraySize()
print n, " coordinates"

#get coordinates
print "getting coordinates"
for i in range(n):
	print "getting coordinate %i"%i
	ret = cont.GetArrayElement(i)
	print ("(%f, %f, %f) isDifferential %d, isPathTarget %d, isWaitPoint %d")%(ret.x,ret.y,ret.a,\
		ret.isDifferential, ret.isPathTarget, ret.isWaitPoint)


l,r = 0. , 0.
print "get path length"
ret = cdc.GetPathLength(l, r)
print "path length get succesfull? ",
print ret[0]
print "linear length:\t%f"%ret[1]
print "rotational length:\t%f"%ret[2]


cdc.Close()

